#pragma config(Motor,  port2,           RightDrive,     tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port3,           LeftDrive,     tmotorNormal, openLoop)
#pragma config(Motor,  port4,           LeftArm1,     tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port5,           RightArm,     tmotorNormal, openLoop)
#pragma config(Motor,  port6,           ArmJoint,     tmotorNormal, openLoop)
#pragma config(Motor,  port7,           LeftArm2,      tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port8,           ClawMotor,     tmotorNormal, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#define MAX_ARM 110
#define MAX_JOINT 100
#define MAX_CLAW 100

#pragma platform(VEX)

//Competition Control and Duration Settings
#pragma competitionControl(Competition)
#pragma autonomousDuration(20)
#pragma userControlDuration(120)

#include "Vex_Competition_Includes.c"   //Main competition background code...do not modify!

/////////////////////////////////////////////////////////////////////////////////////////
//
//                          Pre-Autonomous Functions
//
// You may want to perform some actions before the competition starts. Do them in the
// following function.
//
/////////////////////////////////////////////////////////////////////////////////////////

void pre_auton()
{
	// All activities that occur before the competition starts
	// Example: clearing encoders, setting servo positions, ...
}

/////////////////////////////////////////////////////////////////////////////////////////
//
//                                 Autonomous Task
//
// This task is used to control your robot during the autonomous phase of a VEX Competition.
// You must modify the code to add your own robot specific commands here.
//
/////////////////////////////////////////////////////////////////////////////////////////

task autonomous()
{
  wait10Msec(50);
  motor[RightArm] = 100;
  motor[LeftArm1] = 100;
  wait10Msec(30);
  motor[RightArm] = 0;
  motor[LeftArm1] = 0;
  motor[ArmJoint] = -100;
  wait10Msec(30);
  motor[ArmJoint] = 0;
  motor[ClawMotor] = -100;
  wait10Msec(30);
  motor[ClawMotor] = 0;
}

/////////////////////////////////////////////////////////////////////////////////////////
//
//                                 User Control Task
//
// This task is used to control your robot during the user control phase of a VEX Competition.
// You must modify the code to add your own robot specific commands here.
//
/////////////////////////////////////////////////////////////////////////////////////////


	void tankDrive()
{
  motor[RightDrive] = vexRT[Ch2];
  motor[LeftDrive] = vexRT[Ch3];
}

void armDrive()
{
  if(vexRT[Btn5U] == 1)
  {
    motor[RightArm] = MAX_ARM;
    motor[LeftArm1] = MAX_ARM;
    motor[LeftArm2] = MAX_ARM;
  }
  else if(vexRT[Btn5D] == 1)
  {
    motor[LeftArm1] = -MAX_ARM;
    motor[LeftArm2] = -MAX_ARM;
    motor[RightArm] = -MAX_ARM;

  }
  else
  {
    motor[LeftArm1] = 0;
    motor[LeftArm2] = 0;
    motor[RightArm] = 0;
  }
}

void jointDrive()
{
  if(vexRT[Btn6U] == 1)
  {
    motor[ArmJoint] = MAX_JOINT;
  }
  else if(vexRT[Btn6D] == 1)
  {
    motor
    [ArmJoint] = -MAX_JOINT;
  }
  else
  {
    motor[ArmJoint] = 0;
  }
}

void clawDrive()
{
  if(vexRT[Btn8D] == 1)
  {
    motor[ClawMotor] = MAX_CLAW;
  }
  else if(vexRT[Btn8U] == 1)
  {
    motor[ClawMotor] = -MAX_CLAW;
  }
  else
  {
    motor[ClawMotor] = 0;
  }
}

task usercontrol()
{
  while (true)
  {
    tankDrive();
    armDrive();
    jointDrive();
    clawDrive();
  }
}
